/**
**************************************************************************************
*Founder Effect                                                                      *
*Copyright (C) 2006-2007, 5d Studios (www.5d-Studios.com)                            *
*                                                                                    *
*This library is free software; you can redistribute it and/or                       *
*modify it under the terms of the GNU Lesser General Public                          *
*License as published by the Free Software Foundation; either                        *
*version 2.1 of the License, or (at your option) any later version.                  *
*                                                                                    *
*This library is distributed in the hope that it will be useful,                     *
*but WITHOUT ANY WARRANTY; without even the implied warranty of                      *
*MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU                   *
*Lesser General Public License for more details.                                     *
*                                                                                    *
*You should have received a copy of the GNU Lesser General Public                    *
*License along with this library; if not, write to the Free Software                 *
*Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA      *
**************************************************************************************
*/
#include "StdAfx.h"
#include "CollisionManager.h"
#include "IEntity.h"
#include "IWeapon.h"
#include "Player.h"

#include "../shared/StringUtil.h"
#include "../shared/QuadTree.h"
#include "../render/ViewManager.h"
#include "../kernel.h"

extern Kernel* g_kernel;

#define TIME (g_kernel->GetTime())
#define GRAVITY (20.0f)


CollisionManager::CollisionManager(void)
{
}

CollisionManager::~CollisionManager(void)
{
}


/** Update each entity */
void CollisionManager::Update( ICamera* cam, Scene* scene, type_EntityMap &entities, float deltaTime )
{
    float time = g_kernel->GetTime();     // current time

    type_EntityMap::iterator it = entities.begin();
    for(; it != entities.end(); ++it )
    {
        IEntity* ent = (*it).second;
        ent->m_flags &= ~EF_NOTONSCREEN;        // clear flags for this update
        ent->m_flags &= ~EF_UPDATE;

        if ( !cam->IsMainCamera() ) { 
            if ( cam->IsAttached( ent ) )
                scene->CenterCamera( cam, ent->m_physObject.GetPosition() );
            continue;
        }
 
        // make sure the entity is on screen
        if ( !cam->Contains( ent->GetBounds() ) ) { 
            ent->m_flags |= EF_NOTONSCREEN;
            continue;
        }
        bool popped = m_entityQuadTree->Pop( ent );
        if ( !popped ) g_kernel->LPrintf( ("Not popped: " + ent->GetName()).c_str() );

        if ( time > ent->m_nextUpdate )
        {
            ent->m_flags |= EF_UPDATE;
            ent->Update( deltaTime );
        }
        Move( ent, cam, scene, deltaTime ); // move the entity

        if ( m_entityQuadTree->Pop( ent ) ) {
            g_kernel->LPrintf( "Popped Bounds: %d %d %d %d", ent->GetBounds().x, ent->GetBounds().y, ent->GetBounds().width, ent->GetBounds().height );
            g_kernel->LPrintf( ("*******WARNING::Should not be popping here! Entity Name " + ent->GetName() +" ****************** ").c_str() );
        }

        if ( popped ) m_entityQuadTree->Put( ent );
            
       
    }    
}

/**
===============================
Move each entity -
===============================
*/
bool CollisionManager::Move(IEntity* ent, ICamera* cam, Scene* scene, float dt)
{
    // entity is not moving
    if ( !ent->m_vDirection.Length() ) {
        Vector2f position = ent->m_physObject.GetPosition();
        return CheckCollision( ent, position );
    }

    // Apply gravity if needed
    if ( CheckGravity(ent, dt) )
        return false;

    ent->m_physObject.ApplyImpulse( GetMotion( ent ) );
    ent->m_physObject.ApplyImpulse( Vector2f( 0, ent->m_fGravity ) );

    Vector2f oldPosition = ent->m_physObject.GetPosition();
    
    ent->m_physObject.Update( dt );
    Vector2f moveTo =  ent->m_physObject.GetPosition() - oldPosition;

    // check collisions
    float x = MoveX( ent, moveTo.x, oldPosition, scene );
    float y = MoveY( ent, moveTo.y, oldPosition, scene );
    
    moveTo.Set( x, y );
    ent->m_physObject.SetPosition( moveTo );

    // test to see if we hit a trigger
    scene->InvokeTrigger( ent, moveTo.x, moveTo.y + ent->GetBounds().height/2 );

    // move the camera for the player
    if ( cam->IsAttached( ent ) )
    {
        // center the camera around the player
        scene->CenterCamera( cam, ent->m_physObject.GetPosition() );
    }
    return true;
}


/** Move X coord of an ent */
float CollisionManager::MoveX(IEntity *ent, float xmove, Vector2f &oldPosition, Scene* scene)
{
    Rect bounds = ent->GetBounds();
    int midHeight = bounds.height / 2;

    float xToMove = 0.0f;
    while( xmove )
    {
        // move one pixel
        // and test collision
        if ( xmove > 1 )
        {
            xToMove = 1.0f;
            xmove--;
        } 
        else if ( xmove < -1 )
        {
            xToMove = -1.0f;
            xmove++;
        }
        else
        {
            xToMove = xmove;
            xmove = 0.0f;
        }

        oldPosition.x += xToMove;

        // check collision
        if ( scene->Collide( oldPosition.x, oldPosition.y + midHeight, bounds ) ||
             CheckCollision( ent, oldPosition ) )  
        {
            oldPosition.x -= xToMove;
            ent->UpdateCollisionBounds( oldPosition.x, oldPosition.y );
            return oldPosition.x;
        }

    }
    return oldPosition.x;
}

/** Move Y coord of an ent */
float CollisionManager::MoveY(IEntity *ent, float ymove, Vector2f &oldPosition, Scene* scene)
{
    Rect bounds = ent->GetBounds();
    int midHeight = bounds.height / 2;

    float yToMove = 0.0f;
    while( ymove )
    {
        // move one pixel
        // and test collision
        if ( ymove > 1 )
        {
            yToMove = 1.0f;
            ymove--;
        } 
        else if ( ymove < -1.0f )
        {
            yToMove = -1.0f;
            ymove++;
        } 
        else
        {
            // we hit 1 pixel
            yToMove = ymove;
            ymove = 0;
        }

        // move ent 1 pixel ahead
        oldPosition.y += yToMove;

        // check collision
        if ( scene->Collide( oldPosition.x, oldPosition.y + midHeight, bounds ) ||
             CheckCollision( ent, oldPosition ) )
        {
            oldPosition.y -= yToMove;
            ent->UpdateCollisionBounds( oldPosition.x, oldPosition.y );
            return oldPosition.y;
        }

    }
    return oldPosition.y;
}

/** Check collision between entities 
 *  Should be O(lg(N)) operation now with m_entityQuadTree
 */
bool CollisionManager::CheckCollision(IEntity* ent, const Vector2f &pos )
{
    ent->UpdateCollisionBounds( pos.x, pos.y );
    
    type_EntityMap list;
    m_entityQuadTree->GetObjects( ent->GetBounds(), list );

    type_EntityMap::iterator it = list.begin();
    for (; it != list.end(); ++it )
    {
        IEntity* other = it->second;
        
        // don't collide with yourself
        if ( ent == other ) 
            continue;

        // entity is being erased
        if ( other->CanRemove() )
            continue;

        // if this entity is being carried
        // do not col test
        if ( other->GetState() == ENT_STATES[CARRIED] )
            continue;

        if ( ent->GetState() == ENT_STATES[ THROWN ] )
        {
            // FIXME: Should be something like this: if ( ent->CarriedBy() == other )
            if ( other->GetState() == ENT_STATES[ THROWING ] )
                continue;
        }

        // Collide with eachother
        if ( ent->GetBounds().Intersects( other->GetBounds() ) )
        {
            ent->CollideWith( other );
            other->CollideWith( ent );
            return !(other->m_flags & EF_NOCOLLIDE);
        }
    }
    return false;
}



/** Apply Gravity */
bool CollisionManager::CheckGravity(IEntity* ent, float deltaTime)
{
    // apply gravity -- NEEDS SOME WORK
    if ( ent->m_heightAboveGround )
    {   
        ent->m_fGravity = GRAVITY;
        ent->m_heightAboveGround -= ent->m_fGravity * deltaTime;
        
        if ( ent->m_heightAboveGround <= 0 )
        {
            ent->m_heightAboveGround = 0;
            ent->m_fGravity = 0;
        }
    } 
    else
    {
        ent->m_fGravity = 0;
        ent->m_heightAboveGround = 0;
        if ( ent->m_flags & EF_MOVED )
        {
            ent->m_vDirection.ZeroOut();
            ent->m_flags &= ~EF_MOVED; // we are done moving

            Vector2f position = ent->m_physObject.GetPosition();
            return !CheckCollision( ent, position );
        }
    }
    return false;
}

/** Check type of motion */
Vector2f CollisionManager::GetMotion(IEntity* ent)
{
    float speed = 0.0f;

    int state = ent->GetIntState();
    switch( state )
    {
    case WALKING:
    case CARRYING_WALKING: speed = ent->m_fSpeed;
        break; 
    case PUSHED: speed = ent->m_fPushSpeed;
        break;
    case THROWN: speed = ent->m_fThrowSpeed;
        break;
    default: break;
    }
    return ent->m_vDirection * speed;
}